//===========================================================================
/*!
 *  \file Potential.cc
 */
//===========================================================================

#include "Potential.h"

Potential* Potential::pSelf;

const char* Potential::Equation[]={ "OSS2","OSS2_ITER","TTM2F","TTM21F","TTM3F", "Ghemical","MMFF94","MMFF94s" ,"UFF","TTM21F_ICE","TTM3F_ICE","QSPCFW_ICE",
	"SPCF_ICE","HF_OSS2"};
		

int Potential::genome(double const* x){	
	for(int i=0;i<dim;i++){
		 init_x(i+1)=x[i];		
		 //cout<<init_x(i+1)<<" ";
	}
	//cout<<endl;
}

int Potential::genome(double const* x,int dim_){	
	SetDim_(dim_);	genome(x);
}

void Potential::hessian_(double* _p, double** _gg,double delta){
	int i,j;
	double** d1=AllocateArray(dim,dim);
	double** d2=AllocateArray(dim,dim);
		
	if(HasAnalyticalGradients()){
		for(i=0;i<dim;i++){ //using central finite difference
			_p[i]+=delta;	gradient_(_p,d1[i]);	_p[i]-=delta;			
			_p[i]-=delta;	gradient_(_p,d2[i]);	_p[i]+=delta;
		}
		
		for(i=0;i<dim;i++)
			for(j=i;j<dim;j++){
				//_gg[l][i]=(dfold1[l]-dfold2[l])/(2.0*delta);
			_gg[i][j]=_gg[j][i]=(d1[i][j]-d2[i][j] + d1[j][i]-d2[j][i] )/(4.0*delta);
			}		
	}else{ //using forward finite difference
		double fx=evaluate_(_p);
		double* f1=new double[dim];
		for(i=0;i<dim;i++){
			_p[i]+=delta;
			
			f1[i]=evaluate_(_p);
			
			for(j=i;j<dim;j++){
				_p[j]+=delta;
				d1[i][j]= evaluate_(_p);
				_p[j]-=delta;
			}
			_p[i]-=delta;
		}
		for(i=0;i<dim;i++)
			for(j=i;j<dim;j++){
				_gg[i][j]=_gg[j][i]=(d1[i][j]-f1[i]-f1[j] +fx)/(delta*delta);			
			}
		delete[] f1;
	}	
	FreeArray(d1,dim);
	FreeArray(d2,dim);
}

double Potential::gradient_num(double const* xt, double* grad,double delta){
	double e1,e2;
	double* x=new double[dim];
	for(int i=0;i<dim;i++) x[i]=xt[i];
	double rms=0;
	for(int i=0;i<dim;i++){
		x[i]+=delta;	e1=evaluate_(x);	x[i]-=delta;			
		x[i]-=delta;	e2=evaluate_(x);	x[i]+=delta;
				//cout<<i<<" = "<<e1<<" "<<e2<<" = "<<(e1-e2)/(2.0*delta)<<endl;
		if(grad){
			grad[i]=(e1-e2)/(2.0*delta);
			rms+=pow(grad[i],2);
		}else rms+=pow((e1-e2)/(2.0*delta),2);
	}
	rms=sqrt(rms/dim);
	delete[] x;
	return rms;
};
		

double Potential::ValidateGradients (double *x,int verbose){
	ColumnVector numgrad, anagrad;
	double err=0,maxerr=0;
    
	numgrad.ReSize(dim);
	anagrad.ReSize(dim);
	
	gradient_num(x,numgrad.data());
	if(HasAnalyticalGradients())
		gradient_(x,anagrad.data());
	
	if(verbose>0){
		cout<<"\nValidate Gradients\n\n";
		cout<<"IDX      NUM_GRAD     ANA_GRAD   REL. ERROR (%)   \n";
		cout<<"------------------------------------------------------------\n";
	}
	
	for(int i=0;i<dim;i++){
		if(numgrad[i]!=0) err=fabs((anagrad[i]-numgrad[i])/numgrad[i]);
		else err=0;
		
 		if(verbose>0) cout<<format("%2d    (%+4.6lf )  (%+4.6lf ) ( %6.2f) \n")% i% numgrad[i]%	 anagrad[i]% err ;		
		maxerr=(err<maxerr)?maxerr:err;
	}
	if(verbose>0) cout<<" Maximum error = "<<maxerr<<endl;
		  
	return maxerr; // are all components good enough?
}

int Potential::optimize(double *x,int debug){	
	//setting the variable
	neval=0;		
	
	if(debug>=2){
		cout<<"Dimension = "<<Dim_();
		cout<<" Energy="<<evaluate_(x)<<" Method="<<opt_med<<endl;	
	}
	
	if(opt_med==DFPMIN){
		double *xb=new double[dim];		
		for(int i=0;i<dim;i++)	xb[i]=x[i];
		iConverged=dfpmin(xb,dim,nMaxEval, &final_eval);
		for(int i=0;i<dim;i++)	final_x[i]=xb[i];
		delete[] xb;
		return neval;
	}
	//cout<<(h.checkConvg()?"Converged":"Not Yet")<<endl;
	//cout<<"Time = "<<h.getSeconds()<<endl;
	//cout<<"nEval = "<<h.getNEvals()<<endl;
	
	//start optmization
	genome(x); Potential *pTemp=pSelf;	pSelf=this;
	
	NLF1 nlp(dim,&Potential::evalgenome_1,&Potential::initgenome);	
	
	switch(opt_med){
		default:
			//iConverged=optimize_QNewton(nlp);
			iConverged=optimize_nlf1<OptQNewton>(nlp);
		break;
		case CONJUGATEGRAD :
			//iConverged=optimize_CG(nlp);
			iConverged=optimize_nlf1<OptCG>(nlp);
		break;
		case LBFGS:
			iConverged=optimize_nlf1<OptLBFGS>(nlp);
		break;
		case FDNEWTON:
			iConverged=optimize_nlf1<OptFDNewton>(nlp);
		break;
		
		
	}	
	pSelf=pTemp;  
	
	final_eval=nlp.getF();
	
	final_x= nlp.getXc();
	
	ColumnVector t = nlp.getGrad();
	
	MaxGrad=t.MaximumAbsoluteValue();
	RmsGrad=sqrt(t.SumSquare()/dim);	
	
	t.Release();
	nsecs=nlp.getFcnTime(); // time
	return neval;
	
}

int Potential::optimize(Individual& inv,int debug){	
	SetDim_(inv.nAtom*3);
	int n=optimize(inv.x,debug);
	getFinalEval(inv.energy);
	if(inv.energy==inv.energy){		
		getFinalX(inv.x);
		getRmsGrad(inv.rmsGrad);
	}	
	return n;
}


template<class T>
int Potential::optimize_nlf1(NLF1 &nlp){
	T objfcn(&nlp);
	objfcn.setMaxFeval(nMaxEval);
	objfcn.setFcnTol(FcnTol);	
	objfcn.setGradTol(GradTol);
	objfcn.setMaxStep(MaxStep);
	objfcn.setSearchStrategy(LineSearch);
	objfcn.setMaxBacktrackIter(200);
	if (!objfcn.setOutputFile("OPT_DEFAULT.out", 0)) cerr << "main: output file open failed" << endl;	
	objfcn.optimize();
	objfcn.printStatus("Solution !!");	
	objfcn.cleanup();
	return objfcn.checkConvg(); 
}


#define EPS 1.0e-12
#define TOLX (4*EPS) 
//Convergence criterion on x values.
#define STPMX 2.0 
//Scaled maximum step length allowed in line searches.
#define ALF 1.0e-4 
//Ensures sufficient decrease in function value.
#define SQR(x) (x)*(x)
#define FMAX(a,b) ((a) > (b) ? (a) : (b))

void Potential::lnsrch(int n, double xold[], double fold, double g[], double p[], double x[],double *fret, double stpmax, int *check){
	int i;
	double a,alam,alam2=0,alamin,b,disc,f2=0,rhs1,rhs2,slope,sum,temp,test,tmplam;
	
	*check=0;
	for (sum=0.0,i=0;i<n;i++) sum += p[i]*p[i];
	sum=sqrt(sum);

	if (sum > stpmax)
		for (i=0;i<n;i++) p[i] *= stpmax/sum; //Scale if attempted step is too big.
	for (slope=0.0,i=0;i<n;i++)
		slope += g[i]*p[i];
	if (slope >= 0.0) {};//cout<<"Roundoff problem in lnsrch.\n";
	test=0.0; //Compute min.

	for (i=0;i<n;i++) {
		temp=fabs(p[i])/FMAX(fabs(xold[i]),1.0);
		if (temp > test) test=temp;
	}
	alamin=TOLX/test;
	alam=1.0;  //Always try full Newton step first.
	while (neval < nMaxEval) { //Start of iteration loop.
		for (i=0;i<n;i++) x[i]=xold[i]+alam*p[i];
		*fret=evaluate_(x);
		if (alam < alamin) { //Convergence on x. For zero finding,the calling program should verify the convergence.
			for (i=0;i<n;i++) x[i]=xold[i];
			*check=1;
			return;
		} else if (*fret <= fold+ALF*alam*slope) return; //Sufficient function decrease.
		else { //Backtrack.
			if (alam == 1.0)
				tmplam = -slope/(2.0*(*fret-fold-slope)); //First time.
			else {// Subsequent backtracks.
				rhs1 = *fret-fold-alam*slope;
				rhs2=f2-fold-alam2*slope;
				a=(rhs1/(alam*alam)-rhs2/(alam2*alam2))/(alam-alam2);
				b=(-alam2*rhs1/(alam*alam)+alam*rhs2/(alam2*alam2))/(alam-alam2);
				if (a == 0.0) tmplam = -slope/(2.0*b);
				else {
					disc=b*b-3.0*a*slope;
					if (disc < 0.0) tmplam=0.5*alam;
					else if (b <= 0.0) tmplam=(-b+sqrt(disc))/(3.0*a);
					else tmplam=-slope/(b+sqrt(disc));
				}
				if (tmplam > 0.5*alam)
					tmplam=0.5*alam; 
			}
		}
		alam2=alam;
		f2 = *fret;
		alam=FMAX(tmplam,0.1*alam); 
	}     
    
}

int Potential::dfpmin(double p[], int n,  int iterMax, double *fret ){
	int check,i,its,j,iter;
	double fac,fad,fae,fp,stpmax,sum=0.0,sumdg,sumxi,test=0;	

	double dg[n];
	double g[n];
	double hdg[n];
	double hessin[n][n];
	double pnew[n];
	double xi[n];
	
	fp=evaluate_(p);  //Calculate starting function value and gradient,
	gradient_(p,g);

	for (i=0;i<n;i++) { //and initialize the inverse Hessian to the unit matrix. 
		for (j=0;j<n;j++) hessin[i][j]=0.0;
		hessin[i][i]=1.0;
		xi[i] = -g[i]; //Initial line direction.
		sum += p[i]*p[i];
	}

	stpmax=MaxStep;//*n;
	
	for(iter=0;iter<iterMax;iter++) { //Main loop over the iterations.		
		lnsrch(n,p,fp,g,xi,pnew,fret,stpmax,&check);

		//The new function evaluation occurs in lnsrch; save the function value in fp for the
		//next line search. It is usually safe to ignore the value of check.
		fp = *fret;
		for (i=0;i<n;i++) {
			xi[i]=pnew[i]-p[i]; //Update the line direction,
			p[i]=pnew[i];  // and the current point.
		}	
			
		
		test=0.0; //Test for convergence on x.
		for (i=0;i<n;i++) {
			double temp=fabs(xi[i]);
			if (temp > test) test=temp;
		}	
		if (test < FcnTol)  return 1; 
		
		
		for (i=0;i<n;i++) dg[i]=g[i]; //Save the old gradient,
		gradient_(p,g); //and get the new gradient.
				
		RmsGrad=0; //Test for convergence on zero gradient
		MaxGrad=0; //test for maximum force.			
		for (i=0;i<n;i++) {
			RmsGrad+=g[i]*g[i];
			MaxGrad=FMAX(fabs(g[i]),MaxGrad);
		}
		RmsGrad=sqrt(RmsGrad/n); 
		if ((RmsGrad < GradTol) && (MaxGrad < GradTol*4.0)) return 1;
	
		for (i=0;i<n;i++) dg[i]=g[i]-dg[i]; //Compute difference of gradients,
		for (i=0;i<n;i++) { //and difference times current matrix.
			hdg[i]=0.0;
			for (j=0;j<n;j++) hdg[i] += hessin[i][j]*dg[j];
		}
		fac=fae=sumdg=sumxi=0.0; //Calculate dot products for the denominators.
		for (i=0;i<n;i++) {
			fac += dg[i]*xi[i];
			fae += dg[i]*hdg[i];
			sumdg += SQR(dg[i]);
			sumxi += SQR(xi[i]);
		}
		if (fac > sqrt(EPS*sumdg*sumxi)) { //Skip update if fac not sufficiently positive.
			fac=1.0/fac;
			fad=1.0/fae;
    //The vector that makes BFGS different from DFP:
			for (i=0;i<n;i++) dg[i]=fac*xi[i]-fad*hdg[i];
			for (i=0;i<n;i++) { //The BFGS updating formula:
				for (j=i;j<n;j++) {
					hessin[i][j] += fac*xi[i]*xi[j] -fad*hdg[i]*hdg[j]+fae*dg[i]*dg[j];
					hessin[j][i]=hessin[i][j];
				}
			}
		}
		for (i=0;i<n;i++) { //Now calculate the next direction to go,
			xi[i]=0.0;
			for (j=0;j<n;j++) xi[i] -= hessin[i][j]*g[j];
		}
	}// and go back for another iteration.
	
	return 0;//if overrun	
}
